#include <math.h>
#include <stdlib.h>
#include "FIR.h"
#define GAIN 32768

double	getWindow	(int i, int n, char window ) {
	if( window == BARTLETT ) {
		double a = i - (n - 1) / 2.0f;
		if( a < 0 ) a = -a;
		return 2.0f / n * (n / 2.0f - a);
	}
	else if( window == HANNING )
		return 0.5f - 0.5f * cos(PI / n * (1.0f + 2.0f * i));

	if( window == BLACKMAN )	{
		float a = PI / n * (1.0f + 2.0f * i);
		return 0.5f * (1.0f - 0.16f - cos(a) + 0.16f * cos(2.0f * a));
	}
	else	{
		double a = 2.0f * PI * i / (n - 1);
		if( window == HAMMING )
			return 0.54 - 0.46 * cos( a );
		else if( window == BLACKMAN_HARRIS )
			return 0.35875 - 0.48829 * cos(a) + 0.14128 * cos(2.0 * a) - 0.01168 * cos(3.0 * a);
		else if( window == BLACKMAN_NUTTAL )
			return 0.35819 - 0.4891775 * cos(a) + 0.1365995 * cos(2.0 * a) - 0.0106411 * cos(3.0 * a);
		else if( window == NUTTAL )
			return 0.355768 - 0.487396 * cos(a) + 0.144232 * cos(2.0 * a) - 0.012604 * cos(3.0 * a);
	}
	return 1.0;
}

void FirInit (FIR *fir, char type, char window, short numTaps, int f1, int f2, int sampleRate) {
	int i, n2, d;
	double w, sum;
	double *hf;
	double *coefd;

	fir->type = type;
	fir->window = window;
	fir->numTaps = numTaps;
	fir->coef = (short*)calloc(fir->numTaps, sizeof(short));
	coefd = (double*)calloc(fir->numTaps, sizeof(double));
		
	if( numTaps == 1 ) {
		coefd[0] = 1.0;
		NormalizeCoefs(fir, coefd);
		free(coefd);
		return;
	}
	
	n2 = numTaps / 2;
	w = 2.0f * PI * (double)f1 / (double)sampleRate;
	sum = 0;

	for( i = 0; i < numTaps; i++ ) {
		d = i - n2;
		coefd[i] = ((d == 0) ? w : sin(w * d) / d) * getWindow(i, numTaps, window);
		sum += coefd[i];
	}
	
	for( i = 0; i < numTaps; i++ ) {
		coefd[i] /= sum;
	}
	
	if( type == LOWPASS ){
		NormalizeCoefs(fir, coefd);
		free(coefd);
		return;
	}
	else if( type == HIGHPASS )	{
		for( i = 0; i < numTaps; i++ ) { coefd[i] = -coefd[i]; }
		coefd[n2] += 1.0;
		NormalizeCoefs(fir, coefd);
		free(coefd);
		return;
	}
	else {
		hf = (double*)calloc(fir->numTaps, sizeof(double));
		w = 2.0f * PI * (double)f2 / (double)sampleRate;
		sum = 0;
		for( i = 0; i < numTaps; i++ ) {
			d = i - n2;
			hf[i] = ((d == 0) ? w : sin(w * d) / d) * getWindow(i, numTaps, window);
			sum += hf[i];
		}

		for( i = 0; i < numTaps; i++ ) {
			hf[i] /= sum;
		}
		
		for( i = 0; i < numTaps; i++ ) { 
			coefd[i] -= hf[i]; 
		}
		
		coefd[n2] += 1.0;
		if( type == BANDSTOP ) {
			free(hf);
			NormalizeCoefs(fir, coefd);
			free(coefd);
			return;
		}
		else if( type == BANDPASS )
		{
			for( i = 0; i < numTaps; i++ ) { 
				coefd[i] = -coefd[i];
			}
			coefd[n2] += 1.0;
		}
		free(hf);
		NormalizeCoefs(fir, coefd);
		free(coefd);
	}
}

void NormalizeCoefs(FIR* fir, double* coef) {
	short i;
	for( i = 0; i < fir->numTaps; i++ ) { 
		fir->coef[i] = (q15_t)(coef[i] * GAIN);
	}
}

void FirFree (FIR *fir) {
	free(fir->coef);
}
